#include <ros/ros.h> //ros头文件
#include <sensor_msgs/LaserScan.h> //消息格式的头文件
#include <geometry_msgs/Twist.h>//速度消息格式包


ros::Publisher vel_pub;
int ncount = 0;
void LidarCallback(const sensor_msgs::LaserScan msg)
{
    float fMidDist = msg.ranges[180];
    ROS_INFO("前方测距 ranges[180] = %f 米", fMidDist);
    geometry_msgs::Twist vel_cmd;

    if(ncount > 0)
    {
        ncount --;
        return;
    }
    if(fMidDist < 1.5)
    {
        vel_cmd.angular.z = 0.3;
        ncount = 50;
    }else
    {
        vel_cmd.linear.x = 0.05;
        
    }

    vel_pub.publish(vel_cmd);

}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"zh_CN");//设置中文编码
    ros::init(argc, argv, "lidar_node");//初始化节点

    ros::NodeHandle nh;
    ros::Subscriber lidar_sub = nh.subscribe("/scan", 10, &LidarCallback);
    vel_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel",10);//创建发布的节点和话题
    ros::spin();//让节点保持运行不退出
    
    return 0;
}
